clear all;
close all;
xk1=0;xk2=0;xk3=[0;0];
nu1_1=0;nu1_2=0;nu1_3=0;nu2_1=0;nu2_2=0;nu2_3=0;nu3_1=0;nu3_2=0;sum=0;
T=0.001;
for k=1:1:10000
time(k)=k*T;
if time(k)<5
    Tl(k)=0;
else
    Tl(k)=0.5;
end

tSpan=[0 T];

para1=[nu1_1,nu1_2,nu1_3];      % D/A
[tt,xx]=ode45('iq',tSpan,xk1,[],para1);
xk1=xx(length(xx),:);    % A/D
iq(k)=xk1(1);

para2=[nu2_1,nu2_2,nu2_3];      % D/A
[tt,xx]=ode45('id',tSpan,xk2,[],para2);
xk2=xx(length(xx),:);    % A/D
id(k)=xk2(1);

para3=[nu3_1,nu3_2];      % D/A
[tt,xx]=ode45('plant',tSpan,xk3,[],para3);
xk3=xx(length(xx),:);    % A/D
th(k)=xk3(1);
w(k)=xk3(2);

kk=0.1;gama=0.5;k1=0.7;k2=0.3;k3=0.01;alpha1=2/5;alpha2=1/2;alpha3=2/3;
Rs=1.74;np=4;L=10;fi=0.353;J=0.000724;ro=0.0001;B=0.00000002;

ed=-id(k);
eth=pi-th(k);
deth=-w(k);
ddeth=-3*np*fi*iq(k)/2/J+B*w(k)/J+Tl(k)/J;
sum=sum+(k1*sign(eth)*abs(eth)^alpha1+k2*sign(deth)*abs(deth)^alpha2+k3*sign(ddeth)*abs(ddeth)^alpha3)*T;
v=(Rs+L*B/J)*iq(k)+(np*fi-2*L*B^2/3/np/fi/J)*w(k)-(L*B*Tl(k)/J^2)*2*J/3/np/fi+2*J*L/3/np/fi*(k1*sign(eth)*abs(eth)^alpha1+k2*sign(deth)*abs(deth)^alpha2+k3*sign(ddeth)*abs(ddeth)^alpha3);
s=ddeth+sum;
ud(k)=L*(Rs*id(k)/L-np*w(k)*iq(k)+kk*(sign(ed)*abs(ed)^gama));
uq(k)=v+ro*2*J*L/3/np/fi*sign(s);


nu1_1=id(k);
nu1_2=uq(k);
nu1_3=w(k);
nu2_1=iq(k);
nu2_2=ud(k);
nu2_3=w(k);
nu3_1=iq(k);
nu3_2=Tl(k);
end

figure(1);
plot(time,th,'k');
legend('SMC');
xlabel('time(s)');ylabel('Position');

figure(2);
plot(time,w,'k');
legend('SMC');
xlabel('time(s)');ylabel('Speed');

figure(3);
plot(time,iq,'k');
legend('SMC');
xlabel('time(s)');ylabel('iq');

figure(4);
plot(time,id,'k');
legend('SMC');
xlabel('time(s)');ylabel('id');